// Include files.
#include "AutonomousService.hpp"

AutonomousService::AutonomousService(TestDriveSubsystem &drive) :
	m_thread("FpkTest-Autonomous", &AutonomousService::Run),
	m_drive(RequireControl(drive))
{
}

void AutonomousService::HandleStart()
{
	m_thread.Start(this);
}

void AutonomousService::HandleStop()
{
	m_thread.Stop();
}

void AutonomousService::Run()
{
	m_drive.DriveDistanceByCartesian(0, 5, 0)->Wait();
	m_drive.DriveDistanceByCartesian(0, 5, 0)->Wait();
	m_drive.DriveDistanceByCartesian(0, -5, 0)->Wait();
	m_drive.DriveDistanceByCartesian(0, -5, 0)->Wait();
	m_drive.DriveDistanceByCartesian(-5, 0, 0)->Wait();
	m_drive.DriveDistanceByCartesian(5, 0, 0)->Wait();
	m_drive.DriveDistanceByCartesian(-5, 0, 0)->Wait();
	m_drive.DriveDistanceByCartesian(5, 0, 0)->Wait();
}
